Option explicit
Option default none
'
' Versa Guage Altimeter (R6150V driver)
'
Const i2caddr=&b1110111 ' BMP1800 IC address
Const MS7=7 'set default wait period
Const signed=1
Const unsigned=0
Const buffersize = 2500
Const triangles=128
Const pwm_pin=11 'PWM pin for Display Backlight drive

Dim i2cin$ length 8 'max size for integer conversion
Dim UT%,UP%
Dim As integer ac1%,ac2%,ac3,ac4%,ac5%,ac6%,b1%,b2%,mb%,mc%,md% 'bmp180 parameters
Dim As integer x1%,x2%,b5%,b6%,x3,b3%,b4%,b7%,OSS%
Dim temperature%,pressure%
Dim As float altitude,ALT_old,ALT_avrage,ALT_feet,QNH,pressureinHpa
Dim OSSdata%(4)
Dim OSSscale%(4)

OSS%=1 'set oversampling ratio
OSSdata%(0)=&H34 'commands to sample pressure% with different levels of oversampling
OSSdata%(1)=&H74
OSSdata%(2)=&Hb4
OSSdata%(3)=&HF4
OSSscale%(0)=1 'scale factors for calcs when oversampled
OSSscale%(1)=2
OSSscale%(2)=4
OSSscale%(3)=8

Dim integer i,first=1,offset,buff(buffersize)
Dim integer nt=4 'Number of triangles being updated
Dim integer xx0(nt*2-1),yy0(nt*2-1),xx1(nt*2-1),yy1(nt*2-1),xx2(nt*2-1),yy2(nt*2-1),tcol(nt*2-1)
Dim integer scale1_divisor=1,user_button,touch


I2C OPEN 400,1000 ' IIC Port 400khz, 1sec timeout
SetPin pwm_pin,dout ' Display Backlight
Pin(pwm_pin)=1 ' Just turn ON (for now)

SetPin 23,intl,tst_buttons,pullup ' Select Button
SetPin 30,intl,tst_buttons,pullup ' Up Button
SetPin 31,intl,tst_buttons,pullup ' Down Button

QNH=1000 ' default sea level presure
VAR restore ' last user saved QNH value

CLS ' Clear Display Screen
alt_gauge_face ' Draw ALT Gauge background

I2C WRITE i2caddr,1,1,&HAA 'send read calibration data command
I2C READ i2caddr,0,22,i2cin$() 'read in calibration data
ac1%=intconv(Mid$(i2cin$,1,2),signed)
ac2%=intconv(Mid$(i2cin$,3,2),signed)
ac3=intconv(Mid$(i2cin$,5,2),signed)
ac4%=intconv(Mid$(i2cin$,7,2),unsigned)
ac5%=intconv(Mid$(i2cin$,9,2),unsigned)
ac6%=intconv(Mid$(i2cin$,11,2),unsigned)
b1%=intconv(Mid$(i2cin$,13,2),signed)
b2%=intconv(Mid$(i2cin$,15,2),signed)
mb%=intconv(Mid$(i2cin$,17,2),signed)
mc%=intconv(Mid$(i2cin$,19,2),signed)
md%=intconv(Right$(i2cin$,2),signed)

Do
  update_ALT
  If first Then
    ALT_avrage=altitude
  Else
    ALT_avrage=ALT_avrage*0.9
    Alt_avrage=ALT_avrage+altitude/10
  EndIf
  ALT_feet=Cint(ALT_avrage)
  If ALT_old<>ALT_feet Then
    update_display(ALT_feet,109,110,110)
    ALT_old=ALT_feet
  EndIf
  Print "ALT avrage ="; ALT_avrage
  Print "ALT whole feet ="; ALT_feet
  check_buttons
  Pause 1000
Loop

'Do
'  Input "ALT in feet "; alt_feet
'  update_display(alt_feet,(110-220\12),110,110)
'Loop

'Do
'  update_display(alt_feet,109,110,110)
'  alt_feet=alt_feet+10
'  Pause 100
'Loop

End

Sub update_ALT
  I2C WRITE i2caddr,0,2,&HF4,&H2E 'send temp conversion
  Pause MS7 'wait for temperature% conversion
  I2C WRITE i2caddr,1,1,&HF6 'send read data
  I2C READ i2caddr,0,2,i2cin$() 'read 2 bytes
  UT%=intconv(i2cin$,unsigned)
'  UT%=27898 ' Uncomment this line to check algorithm against datasheet
  I2C WRITE i2caddr,0,2,&HF4,ossdata%(oss%) 'send pressure% conversion
  Pause (oss%+1)*ms7 'wait for the p  ressure% conversion
  I2C WRITE i2caddr,1,1,&HF6 'send read data
  I2C READ i2caddr,0,3,i2cin$() 'read 3 bytes
  UP%=intconv(i2cin$,unsigned)
  UP%=UP%>>(8-oss%) 'scale the oUT%pUT% by the numb%er of unused bits in the xlsb byte
'  UP%=23843' Uncomment this line to check algorithm against datasheet
  calc_temp
  calc_pressure
  pressureinHpa=pressure%/100
'  Print "Temp =";Str$(temperature%/10,4,1);" Deg C"
  Print "Temp =";Str$(temperature%/10*9/5+32,4,1);" Deg F"
  Print "Local pressure =";Str$(pressure%/100,4,1);" Hpa/mb"
'  Input "QNH in Hpa/Mb  ? ",QNH
  altitude=calcaltitude(QNH,pressureinHpa)
  Print "altitude in feet =";altitude
'  Print "Reverse calculate Sea level pressure =";calcQNH(altitude,pressureinHpa);" Hectopascal/mb"
End Sub

' calc_temperature%: calculate the temperature% from the raw temperature% given the calibration parameters
'
Sub calc_temp
  x1%=(UT%-ac6%)*ac5%\powerof2(15)
  x2%=mc%*powerof2(11)/(x1%+md%) 'This needs to be a floating divide to match the datasheet
  b5%=x1%+x2%
  temperature%=(b5%+8)\powerof2(4)
End Sub
'
' calc_pressure: calculate the pressure% from the raw pressure% given the calibration parameters and temperature% oUT%pUT%
'
Sub calc_pressure
   b6%=b5%-4000
   x1%=(b2%*(b6%*b6%/powerof2(12)))\powerof2(11)
   x2%=ac2%*b6%\powerof2(11)
   x3=x1%+x2%
   b3%=(((ac1%*4+x3)*ossscale%(oss%))+2)\4
   x1%=AC3*b6%\POWEROF2(13)
   x2%=(b1%*(b6%*b6%/POWEROF2(12)))\POWEROF2(16)
   x3=((x1%+x2%)+2)\4
   b4%=ac4%*(Abs(x3+32768))\powerof2(15)
   b7%=Abs(UP%-b3%)*(50000\ossscale%(oss%))
   pressure%=(b7%*2)\b4%
   x1%=(pressure%\powerof2(8))*(pressure%\powerof2(8))
   x1%=(x1%*3038)\powerof2(16)
   x2%=(-7357*pressure%)\powerof2(16)
   pressure%=pressure%+(x1%+x2%+3791)\powerof2(4)
   End Sub
'
Function calcQNH(currentaltitude As float,localpressure As float) As float
   calcQNH=(localpressure*100)/((1-(currentaltitude*0.3048/44330))^5.255)/100
End Function
'
Function log10(x As float) As float
  log10=Log(x)/2.302585093
End Function
'
Function calcaltitude(sealevelpressure As float, localpressure As float) As float
  Local a As float, b As float
   calcaltitude=((10^(log10(localpressure/sealevelpressure)/5.2558797))-1) /-6.8755856*1000000
End Function

Sub update_display(value As integer, size As integer, x As integer, y As integer)
    Local integer x1,y1,x2,y2,x0,y0,i
    Local float angle=value*0.36
    rotatetriangle(2,RGB(RED),angle,x,y,-2,20,2,20,-2,-size) 'make up the second hand with two triangles
    rotatetriangle(3,RGB(RED),angle,x,y,2,-size,2,20,-2,-size)
    angle=value*0.036
    rotatetriangle(0,RGB(green),angle,x,y,-size/15,0,size/15,0,0,-size*0.85)
    angle=value*0.0036
    rotatetriangle(1,RGB(yellow),angle,x,y,-size/12,0,size/12,0,0,-size*0.5)
    If first Then
      i=R61505V_V44(triangles Or nt, buff() , tcol(nt), xx0(nt), yy0(nt), xx1(nt), yy1(nt), xx2(nt), yy2(nt))
      Print "Read buffer is ",i/buffersize*100,"% used"
      first=0
    Else
      i=R61505V_V44(triangles Or (nt*2), buff() , tcol(), xx0(), yy0(), xx1(), yy1(), xx2(), yy2())
    EndIf
    Circle x,y, size\12, 0, , RGB(red), RGB(red))
    Circle x,y, size\15, 0, , 0, 0
    Circle x,y, size\20, 0, , RGB(gray), RGB(gray)
End Sub

Sub rotatetriangle(n As integer, col As integer, angle As float, x As integer, y As integer, x0 As integer, y0 As integer, x1 As integer, y1 As integer, x2 As integer, y2 As integer)
   Local float sine=Sin(Rad(angle)),cosine=Cos(Rad(angle))
   Local integer x0a,y0a,x1a,y1a,x2a,y2a
   x0a= x0*cosine - y0 * sine + x
   y0a= y0*cosine + x0 * sine + y
   x1a= x1*cosine - y1 * sine + x
   y1a= y1*cosine + x1 * sine + y
   x2a= x2*cosine - y2 * sine + x
   y2a= y2*cosine + x2 * sine + y
   xx0(n)=xx0(n+nt)
   yy0(n)=yy0(n+nt)
   xx1(n)=xx1(n+nt)
   yy1(n)=yy1(n+nt)
   xx2(n)=xx2(n+nt)
   yy2(n)=yy2(n+nt)
   xx0(n+nt)=x0a
   yy0(n+nt)=y0a
   xx1(n+nt)=x1a
   yy1(n+nt)=y1a
   xx2(n+nt)=x2a
   yy2(n+nt)=y2a
   tcol(n)=-1
   tcol(n+nt)=col
End Sub

Sub ALT_gauge_face
    scale1_divisor=1
    Local float c1,c2,tick_angle
    For c1 = 1 To 10
      needle_1(110,110,97,110,tick_angle,RGB(white))
      tick_angle=tick_angle+7.2
    For c2 = 1 To 4
      needle_1(110,110,105,110,tick_angle,RGB(white))
      tick_angle=tick_angle+7.2
    Next c2
    Next c1
    Text 112,35,"0",cm,5,1,RGB(white),RGB(black)
    Text 157,50,"1",cm,5,1,RGB(white),RGB(black)
    Text 187,85,"2",cm,5,1,RGB(white),RGB(black)
    Text 187,140,"3",cm,5,1,RGB(white),RGB(black)
    Text 152,175,"4",cm,5,1,RGB(white),RGB(black)
    Text 112,190,"5",cm,5,1,RGB(white),RGB(black)
    Text 67,175,"6",cm,5,1,RGB(white),RGB(black)
    Text 35,140,"7",cm,5,1,RGB(white),RGB(black)
    Text 35,85,"8",cm,5,1,RGB(white),RGB(black)
    Text 65,50,"9",cm,5,1,RGB(white),RGB(black)
    Text 112,80,"ALT",cm,2,1,RGB(white),RGB(black)
    Text 112,140,"    ",cm,2,1,RGB(black),RGB(white)
    Text 112,140,Str$(QNH),cm,2,1,RGB(black),RGB(white)
    Text 112,154,"Hpa/mb",cm,1,1,RGB(white),RGB(black)
    Circle 110,110,9,2,1,RGB(white)' create needle bezel
End Sub

Sub needle_1(x As integer, y As integer, size As integer,size2 As integer, angle As integer, col As integer)
Local integer x1,y1,x2,y2,x3,y3,dummy,mul=1,buffer(200),xx0(2),yy0(2),xx1(2),yy1(2),xx2(2),yy2(2),tcol(2)
    If scale1_divisor=0 Then mul=2
    x1=Sin(Rad(angle))*size + x ' draw first triangle
    y1=-Cos(Rad(angle))*size + y
    x2=Sin(Rad(angle))*size2 + x
    y2=-Cos(Rad(angle))*size2 + y
    x3=Sin(Rad(angle+mul))*size2 + x
    y3=-Cos(Rad(angle+mul))*size2 + y

' Load values into memory array

    dummy=R61505V_V44(129, buffer(), col, x1, y1, x2, y2, x3, y3)
    xx0(0)=x1: yy0(0)=y1: xx1(0)=x2: yy1(0)=y2: xx2(0)=x3: yy2(0)=y3: tcol(0)=col

' save position values

'    needle_1_lastangle=angle: Needle_1_lastx1=x1: Needle_1_lastx2=x2
'    Needle_1_lasty1=y1: Needle_1_lasty2=y2

    x1=Sin(Rad(angle))*size + x ' draw second triangle
    y1=-Cos(Rad(angle))*size + y
    x2=Sin(Rad(angle+mul))*size2 + x
    y2=-Cos(Rad(angle+mul))*size2 + y
    x3=Sin(Rad(angle+mul))*size + x
    y3=-Cos(Rad(angle+mul))*size + y

' Load values into array before calling Cfunction

    dummy=R61505V_V44(129, buffer(), col, x1, y1, x2, y2, x3, y3)

    xx0(1)=x1: yy0(1)=y1: xx1(1)=x2: yy1(1)=y2: xx2(1)=x3: yy2(1)=y3: tcol(1)=col
'    dummy=R61505V_V44(triangles Or 2, buff(), tcol(), xx0(), yy0(), xx1(), yy1(), xx2(), yy2())
End Sub

Sub check_buttons
    If touch Then
    Select Case user_button
      Case 23
        VAR save QNH
      Case 30
        QNH=QNH+1
        touch=0
      Case 31
        QNH=QNH-1
        touch=0
    End Select
    EndIf
End Sub

Sub tst_buttons
  If Not touch Then ' touch clear
    If Not Pin(23) And Pin(30) And Pin(31) Then user_button=23
    If Not Pin(30) And Pin(23) And Pin(31) Then user_button=30
    If Not Pin(31) And Pin(23) And Pin(30) Then user_button=31
'    Print Pin(23),Pin(30),Pin(31)
    Print user_button
    touch=1 ' set touch
    Do: Loop Until Pin(23) And Pin(30) And Pin(31) ' all not touched
    Text 112,140,"    ",cm,2,1,RGB(black),RGB(white)
    Text 112,140,Str$(QNH),cm,2,1,RGB(black),RGB(white)
  EndIf
End Sub

CFunction intconv
     00000000
     27bdfff8 00001021 00001821 afa20000 afa30004 90880000 1900000b 01003821
     2503ffff 03a31821 24020001 00823021 90c60000 a0660000 24420001 00e2302a
     10c0fffa 2463ffff 8ca20000 1040000d 03a81021 9042ffff 30420080 10400009
     29020008 10400007 03a81021 27a40008 2403ffff a0430000 24420001 5444fffe
     a0430000 8fa20000 8fa30004 03e00008 27bd0008
End CFunction
'
CFunction powerof2
     00000000
     27bdfff8 00001021 00001821 afa20000 afa30004 8c820000 000218c2 03a32021
     000318c0 00431023 24030001 00431004 a0820000 8fa20000 8fa30004 03e00008
     27bd0008
End CFunction                                                                                                                         